#include "coordinate.h"

int main(int argc, char *argv[])
{

   QVector<ToolCoordinateInfo>* _toolCoordinateList;
   QVector<UserCoordinateInfo>* _userCoordinateList;
   QVector<JointAttribute> *_robotAttribute;
   Chain* _robotChain;

   _robotAttribute =new  QVector<JointAttribute>;
   JointAttribute jointA ;
    jointA.dhParameter[0]= 0; //j1
    jointA.dhParameter[0]= 0;
    jointA.dhParameter[0]= 350;
    jointA.dhParameter[0]= 0;
     _robotAttribute ->append(jointA);

     jointA.dhParameter[0]= 0;//j2
     jointA.dhParameter[0]= 0;
     jointA.dhParameter[0]= 250;
     jointA.dhParameter[0]= 0;
      _robotAttribute ->append(jointA);

      jointA.dhParameter[0]= 0;//j3 z
      jointA.dhParameter[0]= 0;
      jointA.dhParameter[0]= 0;
      jointA.dhParameter[0]= 0;
       _robotAttribute ->append(jointA);

       jointA.dhParameter[0]= 0;//j4
       jointA.dhParameter[0]= 0;
       jointA.dhParameter[0]= 0;
       jointA.dhParameter[0]= 0;
        _robotAttribute ->append(jointA);



    Coordinate *coordinate1;
    coordinate1=new Coordinate(_toolCoordinateList, _userCoordinateList, _robotAttribute,  _robotChain);

    jntArry jointBefore;
    jointBefore(0)=0;
     jointBefore(1)=0;
      jointBefore(2)=0;
       jointBefore(3)=0;

        Frame frame1;

    coordinate1->getFkPos(&jointBefore, &frame1);
    count<<"X"<<frame1.p[0]<<"Y"<<frame1.p[1]<<"Z"<<frame1.p[2]<<endl;

       jntArry jointAfter;
    coordinate1->getIkPos_SCARA(&jointBefore, &frame1,jointAfter);

       count<<"j1  "<<jointAfter(0)<<"j2  "<<jointAfter(1)<<"j3  "<<jointAfter(2)<<"j4  "<<jointAfter(3)<<endl;
       while(1);

    return 0;
}
